/**
  ******************************************************************************
  * File Name          : app_wrapper.cpp
  * Description       : app_wrapper program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <app_wrapper/app_wrapper.h>

#define MOBILE_SPEED 0.05

#define PIX_X_ZERO      690
#define PIX_Y_ZERO      460

#define PIX_STOP_MIN   -10
#define PIX_STOP_MAX    10

#define ARM_IDLE        3

#define TIM_DELAY       10

#define UDER_DIS        0.01    //米

#define PIX2M           0.0006

/*
机械臂初始位姿：[0, 45, 45, 0, -90, -45] 

齐次矩阵:
    0.7071    0.7071   -0.0000    0.1367
    0.7071   -0.7071    0.0000   -0.5518
   -0.0000   -0.0000   -1.0000    0.2885
         0         0         0    1.0000

欧拉角[Roll Pitch Yaw]:   [-3.141586 0.000002 0.785414]
四元数[w x y z]:          [0.0000   -0.9239   -0.3827    0.0000]
*/

/*
系统运行状态    
*/
int _state;

/*
用户输入的钢筋点位置
*/                         
int _target;

/*
移动平台当前的钢筋点位置
*/
int c_target = 0;            

/*
移动平台的运动方向
        1:  前进
       -1:  后退
*/
int _mobile_dir = 1;            

typedef struct
{
    float X;
    float Y;
    float Z;
    float roll;
    float pitch;
    float yaw;
} pose_t;

typedef struct{
    unsigned int id;
    pose_t pose;
}marker_t;

marker_t maker;

int arm_run_status = 0;

double _z_offset = 0;
double _DW_2 = 0;
double _DW_1 = 0;

int _test = 0;

int error = 0;

void MarkerCB(const ar_track_alvar_msgs::AlvarMarker::ConstPtr &msg)
{
    maker.id = msg->id;
    if(maker.id > 0)
    {
        maker.pose.X = msg->pose.pose.position.x - PIX_X_ZERO;
        maker.pose.Y = msg->pose.pose.position.y - PIX_Y_ZERO;
        maker.pose.Z = msg->pose.pose.position.z;
        //printf("id:%d X:%f Z:%f \r\n",maker.id, maker.pose.X, maker.pose.Z);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "app_wrapper");
    ros::NodeHandle nh;
    ros::Rate loop_rate(20);

    unsigned char sub_state = 0;
    unsigned int  tim = 0;

    geometry_msgs::Vector3 _pose;
    std_msgs::UInt16 _gun;
    geometry_msgs::Twist _mobile;

    maker.pose.X = 0;
    maker.pose.Y = 0;
    maker.pose.Z = 0;

    nh.getParam("/app_wrapper/test", _test);


    _target = 0;
    nh.setParam("/app_wrapper/target", _target);

    // 控制移动平台
    ros::Publisher pub_mobile = nh.advertise<geometry_msgs::Twist>("/raw_cmd_vel", 1);

    // 接收视觉定位信息
    ros::Subscriber sub_vision = nh.subscribe("/topic_Alvar", 1, MarkerCB);

    while (ros::ok())
    {
        nh.getParam("app_wrapper/target", _target);
        switch (_state)
        {
/*========================================
            用户命令获取
==========================================*/
            case 0:
                switch(sub_state)
                {
                    case 0:
                        printf("0:设备就绪等待用户任务.\r\n");
                        sub_state++;
                        break;
                    case 1:
                       if((_target>0)&&(_target<=6))
                        {
                            printf("  ->检测到用户任务[%d].\r\n",_target);
                            sub_state = 0;
                            _state = 1;
                            nh.setParam("app_wrapper/state", _state); 
                        }
                        break;
                    default:
                        sub_state = 0;
                        break;
                }   
                break;

/*========================================
            1:移动平台视觉定位
==========================================*/
            case 1:
                switch(sub_state)
                {
                    case 0:
                        printf("1:移动平台开始运动.\r\n");
                        if(_target == 0)
                        {
                            sub_state = 0; 
                            _state = 0;
                            error = 0;
                            nh.setParam("app_wrapper/state", _state);
                        }
                        else
                        {   
                            sub_state++;
                        }
                        break;
                    case 1:
                        // 根据用户输入的目标来控制移动平台的运动方向
                        if (maker.id > _target)
                        {
                            _mobile_dir = -1;
                        }
                        else if (maker.id < _target)
                        {
                            _mobile_dir = 1;
                        }
                        else 
                        {
                            if (maker.pose.X < PIX_STOP_MIN)
                            {
                                _mobile_dir = 1;
                            }
                            else if (maker.pose.X > PIX_STOP_MAX)
                            {
                                _mobile_dir = -1;
                            }
                            else
                            {
                                _mobile_dir = 0;
                                if(error>100)
                                {
                                    error = 0;
                                    sub_state++;
                                }
                                else
                                {
                                    error++;
                                }
                            }
                        }
                        if(error)
                        {
                            _mobile.linear.x = _mobile_dir*0.01;
                        }
                        else
                        {
                            _mobile.linear.x = _mobile_dir*MOBILE_SPEED;
                        }
                        _mobile.angular.z = 0;
                        pub_mobile.publish(_mobile); 
                        break;
                    case 2:
                        printf("  ->[id:(%d) X:%0.4f Y:%0.4f]\r\n",maker.id, maker.pose.X, maker.pose.Y);
                        printf("  ->已经到达位置，准备捆扎钢筋.\r\n");
                        sub_state++;
                        break;
                    case 3:
                        tim++;
                        if(tim > 4*TIM_DELAY)
                        {          
                            sub_state = 0;         
                            tim = 0;     
                            _state = 2;
                            nh.setParam("app_wrapper/state", _state); 
                        }
                        break;
                    default:
                        sub_state = 0;
                        break;
                }
                break;
/*========================================
            2:机械臂钢筋捆扎
==========================================*/
            case 2:
                _state = 3;
                nh.setParam("app_wrapper/state", _state);
                break;
/*========================================
            4:复位平台工作状态
==========================================*/
            case 3:
                switch(sub_state)
                {
                    case 0:
                        _mobile.linear.x = 0;
                        _mobile.angular.z = 0;
                        pub_mobile.publish(_mobile); 
                        sub_state = 0;
                        _state = 0;
                        nh.setParam("app_wrapper/state", _state);
                        _target = 0;
                        nh.setParam("app_wrapper/target", _target);
                        break;
                    default:
                        break;
                }
                break;
            default:
                break;
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
}

